%**********************************************************************************************
%****************************  CHAPTER 3: RIGID-BODY MOTIONS  *********************************
%**********************************************************************************************

function AdT = Adjoint(T)
% Takes T a transformation matrix SE3 
% Returns the corresponding 6x6 adjoint representation [AdT]
% Example Input:
%{
  clear;clc;
  T = [[1,0,0,0]; [0,0,-1,0]; [0,1,0,3]; [0,0,0,1]];
  AdT = Adjoint(T)
%} 
% Output:
% AdT =
%     1     0     0     0     0     0
%     0     0    -1     0     0     0
%     0     1     0     0     0     0
%     0     0     3     1     0     0
%     3     0     0     0     0    -1
%     0     0     0     0     1     0
[m,n]=size(T);
if m==4 && n==4
    [R,p]=TransToRp(T);
    AdT=[R,zeros(3);VecToso3(p)*R,R];
else
    msg = 'Input matrix is the wrong size.';
    error(msg);
end
end

